#include "Conm1ElementCalculator.h"
#include "Utility/Algebra/Point.h"
#include "DBManager/IService/Input/Node/INodeService.h"
#include "DBManager/IService/Input/Element/IElementService.h"
#include "SGFem/Pre/Assembly/Assembly.h"
#include "SGFem/Pre/Assembly/CoordinateProcessor.h"

using SG::Algebra::Matrixd;
using SG::Algebra::Point;
using namespace SG::DataStructure::FEM;
using namespace SG::DBManager;
using SG::FEMSolver::AssemblyEigen;
using SG::FEMSolver::CoordinateProcessor;
using namespace SG::FEMSolver::Element;

void Conm1ElementCalculator::Initialize (SG::DataStructure::FEM::ElementTypes type)
{
    m_type = type;
}

void Conm1ElementCalculator::ComputeMass
(
const SG::DataStructure::FEM::CONM1Data& element,
const SG::DataStructure::FEM::NodeData&  node,
_OUT SG::Algebra::Matrixd&               mass
)
{
    // 单元坐标系下的质量矩阵
    mass = element.FillUperTriangle ();

    const auto eleCoordId = element.m_cId;
    auto dispCoordId = node.m_dispCoord;
    if (0 == eleCoordId && 0 == dispCoordId)
    {
        return;
    }

    const auto& coordProcessor = CoordinateProcessor::GetInstance ();

    // 计算 Basic 坐标系下的节点坐标
    Point point;
    coordProcessor.GetGlobalCoordinate (node, _OUT point, _OUT dispCoordId);

    Matrixd T (3, 3);
    if (eleCoordId > 0)
    {
        // 获取单元坐标系的变换矩阵
        coordProcessor.GetCoordTrans (eleCoordId, point, _OUT T);
    }
    if (dispCoordId > 0)
    {
        // 获取位移输出坐标系的变换矩阵
        Matrixd TG (3, 3);
        coordProcessor.GetCoordTrans (dispCoordId, point, _OUT TG);
        T = T * TG.Transpose ();
    }

    // 组装单元变换矩阵：单元坐标系到位移输出坐标系变换
    auto TEle = SG::Algebra::identity (6);
    TEle.BlockFill (0, 0, T);
    TEle.BlockFill (3, 3, T);

    // 单元质量矩阵变换到位移输出坐标系
    mass = TEle.Transpose () * mass * TEle;
}

void Conm1ElementCalculator::ComputeElementMatries (const SG::DBManager::DBServiceFactorySP dbServiceFactory)
{
    auto& assembly = AssemblyEigen::GetInstance ();

    CONM1Data        element;
    std::size_t      nodeIndex;
    std::vector<int> dofGlobalIndexSet (6);
    NodeData         node;
    static const DofSet dofSet{DOFDirections::DOF_Ux,   DOFDirections::DOF_Uy,   DOFDirections::DOF_Uz,
                               DOFDirections::DOF_Rotx, DOFDirections::DOF_Roty, DOFDirections::DOF_Rotz};
    Matrixd mass (6, 6);

    const auto& nodeService = dbServiceFactory->get<INodeService> ();
    const auto& eleService  = dbServiceFactory->get<IElementService> ();
    const auto  eleSize     = eleService->GetElementSize (m_type);

    for (std::size_t iEle{0}; iEle < eleSize; ++iEle)
    {
        eleService->FindByIndex (iEle, _OUT element);

        // 获取单元节点对应的索引
        assembly.FindNodeIndex (element.m_g[0], _OUT nodeIndex);

        // 获取单元自由度对应的全局自由度索引
        assembly.GetGlobalDOFId ({nodeIndex}, dofSet, _OUT dofGlobalIndexSet);

        // 获取单元节点数据
        nodeService->FindByIndex (nodeIndex, _OUT node);

        // 计算位移输出坐标系单元质量矩阵，并累加到总体质量矩阵
        ComputeMass (element, node, _OUT mass);
        assembly.AccumulateCoupledMass (dofGlobalIndexSet, mass);
    }
}